#include "AABBTree.h"

#include "math/math.h"
#include <cassert>

namespace physicsworks {

using math::real_traits;
using math::vec3;

// Class used to sort the faces with respect of centroid projections	
class CompareCentroids {
private:
	int axisId;
public:
	CompareCentroids(int a) : axisId(a) {}
	bool operator () (const FaceCache& face0, const FaceCache& face1) const {
		
		return (face0.centroid(axisId) < face1.centroid(axisId)) ? true : false;
	}
};

AABBTree::AABBTree(vec3* vertices, int numVertices, const int* verticesIndices, int numFaces)
 : geometry(vertices, numVertices, verticesIndices, numFaces)
{
	// compute the triangle centroid of all faces
	FaceCache* fc = new FaceCache[geometry.numFaces];
	for (int i = 0; i < geometry.numFaces; ++i) {
		fc[i].face = &geometry.faces[i];
		fc[i].centroid = math::triangleCentroid(*geometry.faces[i].v0, *geometry.faces[i].v1, *geometry.faces[i].v2);
	}
	
	topDownConstruction(&root, fc, geometry.numFaces);
	delete [] fc;
}

AABBTree::~AABBTree()
{
	delete root;
}

/**
 * Build the tree using a top-down construction.
 */
void AABBTree::topDownConstruction(AABBTreeNode** tree, FaceCache* faces, int numFaces) const
{
	assert(numFaces > 0);
	
	if (numFaces == 1) {
		AABBTreeLeaf* n = new AABBTreeLeaf;
		
		n->box = computeBoundingVolume(faces, numFaces);
		n->face = faces->face;
		n->leftChild = n->rightChild = 0;
		*tree = n;
		
		return;
	}
	
	AABBTreeNode* n = new AABBTreeNode;
	
	*tree = n;
	
	n->box = computeBoundingVolume(faces, numFaces);
	int k = computePartition(faces, numFaces, n->box);
	
	topDownConstruction(&(n->leftChild), faces, k);
	topDownConstruction(&(n->rightChild), &faces[k], numFaces - k);
}

/**
 * Computes the axis' partition, sort the faces and returns the number of faces
 * from first segment of axis' partition.
 */
int AABBTree::computePartition(FaceCache* faces, int numFaces, const AABox& bv) const
{
	// choose the axis x, y or z that has the most
	// distant points
	int axisId;
	if (bv.max.x - bv.min.x > bv.max.y - bv.min.y) {
		if (bv.max.x - bv.min.x > bv.max.z - bv.min.z)
			axisId = 0;
		else
			axisId = 2;
	} else {
		if (bv.max.y - bv.min.y > bv.max.z - bv.min.z)
			axisId = 1;
		else
			axisId = 2;
	}
	
	real sum = 0;
	for (int i = 0; i < numFaces; ++i) {
		sum += faces[i].centroid(axisId);
	}
	real mean = sum / numFaces; // mean of the centroid coordinates

	std::sort(faces, &faces[numFaces], CompareCentroids(axisId));
	
	int half = numFaces / 2;
	FaceCache* fcptr = faces + half;
	while (half > 0) {
		half /= 2;
		if (fcptr->centroid(axisId) < mean)
			fcptr += half;
		else
			fcptr -= half;
	}
	
	return (fcptr->centroid(axisId) < mean) ? fcptr - faces + 1 : fcptr - faces;
}

/**
 * Computes the max and min vectors to produce the bounding box.
 */
AABox AABBTree::computeBoundingVolume(FaceCache* faces, int numFaces) const
{
	AABox bv;
	
	bv.max(-real_traits<real>::max, -real_traits<real>::max, -real_traits<real>::max);
	bv.min(real_traits<real>::max, real_traits<real>::max, real_traits<real>::max);
	
	for (int i = 0; i < numFaces; ++i) {
		
		vec3 min, max;
		
		faces[i].face->minMaxVerts(&min, &max);
		for (int k = 0; k < 3; ++k) {
			
			if (min(k) < bv.min(k))
				bv.min(k) = min(k);
			if (max(k) > bv.max(k))
				bv.max(k) = max(k);
			
		}
	}
	
	return bv;
}

}
